#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

class ImageConverter {
private:
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    image_transport::Subscriber depth_sub_;
    image_transport::Publisher image_pub_;

public:
    ImageConverter() : it_(nh_) {
        // 订阅输入的RGB图像话题
        image_sub_ = it_.subscribe("/camera/color/image_raw", 1, &ImageConverter::imageCb, this);
        // 订阅输入的深度图像话题
        depth_sub_ = it_.subscribe("/camera/depth/image_rect_raw", 1, &ImageConverter::depthCb, this);

        // 发布输出图像话题
        image_pub_ = it_.advertise("/image_converter/output_video", 1);
    }

    ~ImageConverter() {}

    void imageCb(const sensor_msgs::ImageConstPtr& msg) {
        cv_bridge::CvImagePtr cv_ptr;
        try {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        } catch (cv_bridge::Exception& e) {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }

        // 显示图像
        cv::imshow("View", cv_ptr->image);
        cv::waitKey(3);
    }

    void depthCb(const sensor_msgs::ImageConstPtr& msg) {
        cv_bridge::CvImagePtr cv_ptr;
        try {
            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
        } catch (cv_bridge::Exception& e) {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
        }

        // 将深度图像转换为可视化格式
        cv::Mat depth_image = cv_ptr->image;
        cv::normalize(depth_image, depth_image, 0, 1, cv::NORM_MINMAX);
        cv::imshow("Depth View", depth_image);
        cv::waitKey(3);
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "image_converter");
    ImageConverter ic;
    ros::spin();
    return 0;
}